import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import serial
import json

class SerialReaderNode(Node):
    def __init__(self):
        super().__init__('serial_reader_node')
        # 发布到 websocket_cmd 话题（与WebSocket节点统一）
        self.publisher = self.create_publisher(String, 'websocket_cmd', 10)
        # 串口配置
        self.serial_port = serial.Serial(
            '/dev/ttyAMA0', 
            115200, 
            timeout=0.1,
            bytesize=serial.EIGHTBITS,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE
        )
        # 定时读取串口数据
        self.timer = self.create_timer(0.01, self.read_and_publish)
        self.get_logger().info("串口节点启动，发布话题: websocket_cmd（统一格式）")

    def read_and_publish(self):
        if self.serial_port.in_waiting > 0:
            try:
                # 读取串口原始数据（格式：0_0_0）
                data = self.serial_port.readline().decode('utf-8', errors='ignore').strip()
                if not data:
                    return
                self.get_logger().info(f"串口原始数据: {data}")

                # 解析 0_0_0 格式为JSON（与WebSocket统一）
                parts = data.split('_')
                if len(parts) != 3:
                    self.get_logger().error(f"格式错误（需id_channel_pulse）: {data}")
                    return
                try:
                    # 转换为 {"b":板ID, "c":通道, "p":脉宽} 格式
                    cmd_dict = {
                        "b": parts[0],
                        "c": parts[1],
                        "p": int(parts[2])
                    }
                    # 封装为列表（与WebSocket格式对齐）
                    json_cmd = json.dumps([cmd_dict])

                    # 发布到统一话题
                    msg = String()
                    msg.data = json_cmd
                    self.publisher.publish(msg)
                    self.get_logger().info(f"转换后发布: {json_cmd}")

                except ValueError as e:
                    self.get_logger().error(f"数值转换错误: {e}")

            except Exception as e:
                self.get_logger().error(f"串口读取错误: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = SerialReaderNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()